
uniform vec4 rootData[<ROOT_DATA_SIZE>];
uniform vec4 boneData[<BONE_DATA_SIZE>];
uniform vec4 animationData[<ANIMATION_DATA_SIZE>];

uniform vec3 jointList[<JOINT_LIST_SIZE>];

uniform sampler2D meshTexture;

uniform float width;
uniform float height;
uniform sampler2DRect crowdPosTexture;

varying vec3 eyeNormal;
varying vec3 eyePosition;

varying vec3 rootTranslationDebug;

varying vec4 vClosestJointIdList;
varying float vClosestJointId;
varying vec3 vJointColor;

float jointDistances[<JOINT_LIST_SIZE>];



int getNumBones() {
	return boneData[0].r;
}

int getBoneSize() {
	return 3; // 1 id(RGBA) + 2 RGBAs
}

int getRootSize() {
	return 2;
}

int getFrameSize() {
	int frameSize = getNumBones() * getBoneSize();
	frameSize += getRootSize();
	return frameSize;
}



int getBoneDefinitionSize() {
	return 11;
}


int getParentJointId(int jointId) {
	int parentJointId = -1;
	
	if(jointId != -1 && jointId != 0) {
		parentJointId = int(boneData[(jointId - 1) * getBoneDefinitionSize() + 1].g);
	}
	
	return parentJointId;
}


int getChildJointId(vec4 vertex, int jointId) {
	int childJointId = -1;
	
	float minDistance = 999.0;
	
	int currJointId = -1;
	int parentJointId = -1;
	
	if(jointId != -1) {
		
		for(int i = 0; i < <BONE_DATA_SIZE>; i++) {
			parentJointId = int(boneData[i*getBoneDefinitionSize()+1].g);
			
			if(jointId == parentJointId) {
				currJointId = int(boneData[i*getBoneDefinitionSize()+1].r);
				
				if(distance(vertex.xyz, jointList[currJointId]) < minDistance) {
					minDistance = distance(vertex.xyz, jointList[currJointId]);
					childJointId = currJointId;
				}
				
			}
			
		}
		
	}
	
	return childJointId;
}


int getClosestJointId(vec4 vertex) {
	int closestJointId = 0;
	float minDistance = distance(vertex.xyz, jointList[closestJointId]);
	// jointDistances[0] = minDistance;
	
	float currDistance = 9999.0;
	for(int i = 1; i < <JOINT_LIST_SIZE>; i++) {
		
		currDistance = distance(vertex.xyz, jointList[i]);
		// jointDistances[i] = currDistance;
		
		if(currDistance < minDistance) {
			minDistance = currDistance;
			closestJointId = i;
		}
		
	}
	return closestJointId;
}



vec4 getClosestJointIdList(vec4 vertex) {
	vec4 closestJoints = vec4(-1.0,-1.0,-1.0,-1.0);
	
	int currentJointId = getClosestJointId(vertex);
	int parentJointId = -1;
	int childJointId = -1;
	
	closestJoints[0] = currentJointId;
	
	parentJointId = getParentJointId(currentJointId);
	childJointId = getChildJointId(vertex, currentJointId);
	
	if(parentJointId != -1 && childJointId != -1) {
		if(distance(vertex.xyz, jointList[parentJointId]) < distance(vertex.xyz, jointList[childJointId])) {
			closestJoints[1] = parentJointId; 
			closestJoints[2] = childJointId; 
		} else {
			closestJoints[2] = parentJointId; 
			closestJoints[1] = childJointId; 
		}
		
	} else if(childJointId != -1) {
		closestJoints[1] = childJointId; 
	} else {
		closestJoints[1] = parentJointId; 
	}
	
	return closestJoints;
}


vec4 getClosestJointWeightList(vec4 vertex, vec4 closestJointIdList) {
	vec4 closestJointWeightList = vec4(0.0,0.0,0.0,0.0);
	
	float distSum = 0.0;
	
	for(int i = 0; i < 4; i++) {
		if(closestJointIdList[i] != -1) {
			distSum += (distance(vertex.xyz, jointList[int(closestJointIdList[i])]));
		}
	}
	
	for(int i = 0; i < 4; i++) {
		if(closestJointIdList[i] == -1) {
			closestJointWeightList[i] = 0.0;
		} else {
			closestJointWeightList[i] = (distance(vertex.xyz, jointList[int(closestJointIdList[i])]))/distSum;
		}
	}
	
	return closestJointWeightList;
}


mat4 getTranslationMatrix(vec3 direction) {
	float w = 1.0;
	return mat4(
		1.0,			0.0,			0.0,			0.0,
		0.0,			1.0,			0.0,			0.0,
		0.0,			0.0,			1.0,			0.0,
		w*direction.x,	w*direction.y,	w*direction.z,	1.0
	);
}

mat4 getXRotationMatrix(float value, int jointId) {
	
	float chi = -radians(value);
	
	if(jointId >= 17) {
		chi = -chi;  // TODO: upper limbs need opposite direction, fix this!
	}
	
	return mat4(
		1, 	0, 				0, 				0,
		0, 	cos(chi), 		-sin(chi), 		0,
		0,	sin(chi),		cos(chi),		0,
		0,	0,				0,				1
	);
}


mat4 getYRotationMatrix(float value, int jointId) {
	
	float phi = -radians(value);
	
	return mat4(
		cos(phi), 		0, 		sin(phi), 	0,
		0, 				1, 		0, 			0,
		-sin(phi),		0,		cos(phi),	0,
		0,				0,		0,			1
	);
}



mat4 getZRotationMatrix(float value, int jointId) {
	
	float theta = -1.0*radians(value);
	
	return mat4(
		cos(theta), 	-sin(theta), 	0, 	0,
		sin(theta), 	cos(theta), 	0, 	0,
		0,				0,				1,	0,
		0,				0,				0,	1
	);
}




float getAnimationValue(int jointId, int dataIndex) {
	float value = 0.0;
	int id = 0;
	
	// get value first ignoring dof
	if(jointId == 0) {
		// root
		if(dataIndex < 3) {
			value = animationData[0][dataIndex];
		} else {
			value = animationData[1][dataIndex - 3];
		}
	} else {
		// bone
		if(dataIndex < 3) {
			
			for(int i = 0; i < getNumBones(); i++) {
				id = animationData[getBoneSize()*(i)+getRootSize()][0];
				if(id == jointId) {
					value = animationData[getBoneSize()*(i)+getRootSize()+1][dataIndex];
				}
			}
		} else {
			
			for(int i = 0; i < getNumBones(); i++) {
				id = animationData[getBoneSize()*(i)+getRootSize()][0];
				if(id == jointId) {
					value = animationData[getBoneSize()*(i)+getRootSize()+2][dataIndex-3];
				}
			}
		}
	}
	
	return value;
}


mat4 getTranslationMatrixByDof(int dof, int jointId, int dataIndex, float weight) {

	mat4 transformationMatrix = mat4(1.0); // identity matrix by default
	
	float value = weight*getAnimationValue(jointId, dataIndex);
	
	// get correct transformation matrix according to dof
	if(dof == 1) {
		// TX
		transformationMatrix = getTranslationMatrix(vec3(value, 0.0, 0.0));
	} else if(dof == 2) {
		// TY
		transformationMatrix = getTranslationMatrix(vec3(0.0, value, 0.0));
	} else if(dof == 3) {
		// TZ
		transformationMatrix = getTranslationMatrix(vec3(0.0, 0.0, value));
	}
	
	return transformationMatrix;

}


mat4 getTransformationMatrixByDof(int dof, int jointId, int dataIndex) {
	mat4 transformationMatrix = mat4(1.0); // identity matrix by default
	
	float value = getAnimationValue(jointId, dataIndex);
	
	// get correct transformation matrix according to dof
	/*if(dof == 1) {
		// TX
		transformationMatrix = getTranslationMatrix(vec3(value, 0.0, 0.0));
	} else if(dof == 2) {
		// TY
		transformationMatrix = getTranslationMatrix(vec3(0.0, value, 0.0));
	} else if(dof == 3) {
		// TZ
		transformationMatrix = getTranslationMatrix(vec3(0.0, 0.0, value));
	} else*/ if(dof == 4) {
		// RX
		transformationMatrix = getXRotationMatrix(value, jointId);
	} else if(dof == 5) {
		// RY
		transformationMatrix = getYRotationMatrix(value, jointId);
	} else if(dof == 6) {
		// RZ
		transformationMatrix = getZRotationMatrix(value, jointId);
	}
	
	return transformationMatrix;
}



mat4 getTransformationMatrix(int jointId) {
	mat4 transformationMatrix;
	
	if(jointId == 0) {
		// root
		
		transformationMatrix = getTransformationMatrixByDof(rootData[0][0], jointId, 0);
		
		for(int i = 1; i < 6; i++) {
			if(i < 3) {
				transformationMatrix = getTransformationMatrixByDof(rootData[0][i], jointId, i) * transformationMatrix;
			} else {
				transformationMatrix = getTransformationMatrixByDof(rootData[1][i-3], jointId, i) * transformationMatrix;
			}
		}
		
		
	} else {
		// bone
		
		transformationMatrix = getTransformationMatrixByDof(boneData[(jointId - 1) * getBoneDefinitionSize()+6][0], jointId, 0);
		
		for(int i = 1; i < 6; i++) {
			if(i < 3) {
				transformationMatrix = getTransformationMatrixByDof(boneData[(jointId - 1) * getBoneDefinitionSize()+6][i], jointId, i) * transformationMatrix;
			} else {
				transformationMatrix = getTransformationMatrixByDof(boneData[(jointId - 1) * getBoneDefinitionSize()+7][i-3], jointId, i) * transformationMatrix;
			}
		}
		
	}
	
	return transformationMatrix;
}




int initTransformedJointList(int jointId, inout vec4[<JOINT_LIST_SIZE>] transformedJointList) {

	int parentJointId = getParentJointId(jointId);
	
	int i = 0;
	transformedJointList[i] = vec4(jointList[jointId], jointId);
	int numJoints = 1;
	
	for(i = 1; i < <JOINT_LIST_SIZE>; i++) {
		
		if(parentJointId == -1) {
			transformedJointList[i] = vec4(0.0, 0.0, 0.0, parentJointId);
		} else {
			transformedJointList[i] = vec4(jointList[parentJointId], parentJointId);
			numJoints++;
		}
		
		parentJointId = getParentJointId(parentJointId);
	}
	
	return numJoints;

}


mat4 getJointTransformationMatrix(vec3 origin, int jointId) {
	 
	mat4 translation1 = getTranslationMatrix(-origin);
	mat4 translation2 = getTranslationMatrix(origin);
	
	mat4 transformationMatrix = getTransformationMatrix(jointId);
	
	
	return translation2 * transformationMatrix * translation1;
}



vec4 tranformPoint(vec4 vertex, int i, vec3 origin, int jointId, inout vec4 [<JOINT_LIST_SIZE>] transformedJointList) {
	mat4 transformationMatrix = getJointTransformationMatrix(origin, jointId);
	
	
	for(int j = 0; j < i; j++) {
		transformedJointList[j] = vec4((transformationMatrix*vec4(transformedJointList[j].xyz, 1.0)).xyz, transformedJointList[j].w);
	}
	
	
	return transformationMatrix * vertex;
}



vec4 transformVertex(vec4 vertex, int jointId, float weight) {

	if(jointId == -1) {
		return vec4(0.0,0.0,0.0,0.0);
	} else {
	
		vec3 origin = vec3(0.0,0.0,0.0);
		vec4 transformedVertex = vertex;
		
		vec4 transformedJointList[<JOINT_LIST_SIZE>]; // (xyz)->pos, w->id
		
		int numJoints = initTransformedJointList(jointId, transformedJointList);
		
		for(int i = numJoints - 1; i > 0; i--) {
		
			transformedVertex = tranformPoint(transformedVertex, i, origin, transformedJointList[i].w, transformedJointList);
			origin = transformedJointList[i-1].xyz;
		}
		
		transformedVertex = tranformPoint(transformedVertex, 0, origin, transformedJointList[0].w, transformedJointList);
		
		return weight*transformedVertex;
	}
}



void initJointDistances(vec4 vertex) {
	for(int i = 0; i < <JOINT_LIST_SIZE>; i++) {
		jointDistances[i] = distance(vertex.xyz, jointList[i]);
	}
}


int getNumJoints(vec4 closestJointList) {
	int numJoints = 0;
	
	for(int i = 0; i < 4; i++) {
		if(closestJointList[i] != -1) {
			numJoints++;
		}
	}
	
	return numJoints;
}


float getAngleBetween(vec3 a, vec3 b) {
	// dot(a, b) = length(a) * length(b) * cos(theta);
	float theta = acos(dot(a,b)/(length(a)*length(b)));
	return theta;
}


void main(void)
{ 
   vec4 vertex;
   vec3 normal;    

   vertex = gl_Vertex;
   normal = gl_Normal;  
   
   gl_TexCoord[0] = gl_MultiTexCoord0;
   
   
   // initJointDistances(vertex);
   
   // vec4 closestJointIdList = getClosestJointIdList(vertex);
   // vClosestJointIdList = closestJointIdList;
   vClosestJointId = getClosestJointId(vertex);
   
   if(vClosestJointId == 4) {
		vJointColor = vec3(0.0,255.0,0.0);
   } else if(vClosestJointId == 5) {
		vJointColor = vec3(255.0,255.0,0.0);
   } else {
		vJointColor = vec3(0.0,0.0,255.0);
   }
   
   // vec4 closestJointWeightList = getClosestJointWeightList(vertex, closestJointIdList);
   
   vec4 transformedVertex = vec4(0.0,0.0,0.0,0.0);
   
   
   
   // transformedVertex = vertex;
   
   // transformedVertex += transformVertex(vertex, 0, 1.0);
   
   // transformedVertex += transformVertex(vertex, closestJointIdList[0], 1.0);
   
   int parentJointId = getParentJointId(vClosestJointId);
   int childJointId = getChildJointId(vertex, vClosestJointId);
   
   
   if(parentJointId != -1 && childJointId != -1) {
		vec3 p = jointList[parentJointId] - jointList[vClosestJointId];
		vec3 c = jointList[childJointId] - jointList[vClosestJointId];
		vec3 v = vertex.xyz - jointList[vClosestJointId];
		
		float angleVC = getAngleBetween(v, c);
		float angleVP = getAngleBetween(v, p);
		
		if(angleVC < angleVP) {
			transformedVertex = transformVertex(vertex, vClosestJointId, 1.0);
		} else {
			transformedVertex = transformVertex(vertex, parentJointId, 1.0);
		}
		
		
   } else if(parentJointId != -1) {
		transformedVertex = transformVertex(vertex, parentJointId, 1.0);
   } else {
	// childJointId != -1
		transformedVertex = transformVertex(vertex, vClosestJointId, 1.0);
   }
   
   
   // transformedVertex = transformVertex(vertex, vClosestJointId, 1.0);
   
   // transformedVertex = vec4(vertex.xyz + jointList[closestJointIdList[0]], 1.0);
   
   // transformedVertex = vec4(jointList[closestJointIdList[0]], 1.0);
   
   // transformedVertex = vec4(jointList[int(vClosestJointId)], 1.0);
   
   // transformedVertex = vertex;
   /*
   int numJoints = getNumJoints(closestJointList);
   
   if(numJoints == 2) {
		transformedVertex = transformVertex(vertex, vClosestJointId, 1.0);
   } else if(numJoints == 3) {
		
   }
   */
 
	/*
   for(int i = 0; i < 4; i++) {
		
		if(closestJointIdList[i] != -1) {
			transformedVertex += transformVertex(vertex, closestJointIdList[i], closestJointWeightList[i]);
		}
   }
   */
  
	// getTranslationMatrixByDof(int dof, int jointId, int dataIndex)
	mat4 tx = getTranslationMatrixByDof(1, 0, 0, 0.1);
	
	if(gl_InstanceID == 1) {
		tx = getTranslationMatrixByDof(1, 0, 0, -0.1);
	}
	
	mat4 ty = getTranslationMatrixByDof(2, 0, 1, 0.1);
	mat4 tz = getTranslationMatrixByDof(3, 0, 2, 0.1);
	
	if(gl_InstanceID == 3) {
		tz = getTranslationMatrixByDof(3, 0, 2, -0.1);
	}
  
   
   // transformedVertex = vertex;
   
   
   gl_Position = gl_ModelViewProjectionMatrix * tx * ty * tz * transformedVertex;    
   // gl_Position = gl_ModelViewProjectionMatrix * vertex;    
   
   mat4 normalMatrix = gl_ModelViewProjectionMatrix * tx * ty * tz;
   
   mat3 resultant = mat3(
		normalMatrix[0].x, normalMatrix[0].y, normalMatrix[0].z,
		normalMatrix[1].x, normalMatrix[1].y, normalMatrix[1].z,
		normalMatrix[2].x, normalMatrix[2].y, normalMatrix[2].z
   );
   
   resultant = transpose(inverse(resultant));
   
   eyeNormal = resultant * normal;
    
   eyePosition = vec3(0,0,10); 
   
}